#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point, PointStamped, PoseWithCovarianceStamped, Quaternion, PoseStamped, PoseArray
from nav_msgs.msg import Path
from nav_msgs.srv import GetPlan, GetPlanRequest
from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseAction, MoveBaseGoal
from actionlib_msgs.msg import GoalID, GoalStatusArray
from std_msgs.msg import String
from actionlib import SimpleActionClient, GoalStatus
import yaml
import numpy as np
from PyQt5.QtCore import QTimer, pyqtSlot


class TASK_STATUS():
    START = 1
    PAUSE = 2
    STOP = 3
    IDLE = 4


class ROBOT_STATUS():
    ROBOT_BROADCAST = 1
    ROBOT_MOVING = 2
    ROBOT_IDLE = 3
    ROBOT_FINISH = 4


def matrix(q):
    # 四元数转旋转矩阵
    w, x, y, z = q
    rotation_matrix = np.array([
        [1 - 2 * (y ** 2 + z ** 2), 2 * (x * y - z * w), 2 * (x * z + y * w)],
        [2 * (x * y + z * w), 1 - 2 * (x ** 2 + z ** 2), 2 * (y * z - x * w)],
        [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x ** 2 + y ** 2)]
    ])
    return rotation_matrix


class TargetPoint:
    def __init__(self, x, y, z, quat_x, quat_y, quat_z, quat_w, message):
        self.x = x
        self.y = y
        self.z = z
        self.quat_x = quat_x
        self.quat_y = quat_y
        self.quat_z = quat_z
        self.quat_w = quat_w
        self.message = message


def load_targets(path):
    with open(path, 'r') as f:
        target_points_config = yaml.safe_load(f)
        rospy.loginfo("target_points_config loaded", target_points_config)
        m_target_point_maps_ = {}
        targets = target_points_config["targets"]
        for target in targets:
            x = target["value"][0]
            y = target["value"][1]
            z = target["value"][2]
            angle_x = target["value"][3]
            angle_y = target["value"][4]
            angle_z = target["value"][5]
            angle_w = target["value"][6]
            message = target["message"]
            m_target_point_maps_[target["name"]] = TargetPoint(x, y, z, angle_x, angle_y, angle_z, angle_w, message)

        rospy.loginfo("m_target_point_maps_: ", m_target_point_maps_)
        return m_target_point_maps_


target_quat = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)


class NavControl:
    m_target_point_maps_ = {}

    def __init__(self, plainTextEdit):
        self.plainTextEdit = plainTextEdit
        rospy.init_node('marker_publisher', anonymous=True)

        self.m_visualization_markers_pub_ = rospy.Publisher('/target_points', Marker, queue_size=10)
        self.m_global_path_pub_ = rospy.Publisher('/global_path', Path, queue_size=10)
        self.l_path_pub = rospy.Publisher("/move_base_node/NavfnROS/plan", Path, queue_size=10)
        self.m_global_path_ = Path()
        self.m_navigation_lists_ = []
        self.m_goal_pub_ = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=10)
        self.m_goal_status_sub_ = rospy.Subscriber("/move_base/status", GoalStatusArray, self.goal_status_callback,
                                                   queue_size=10)

        self.l_marker_pub_ = rospy.Publisher('visualization_marker', Marker, queue_size=1)
        # self.l_marker_pub_ = nh_.advertise < visualization_msgs::Marker > ("visualization_marker", 1);

        # 订阅工具栏的Publish Point话题
        self.l_clicked_point_sub = rospy.Subscriber("/clicked_point", PointStamped, self.handleClickedPoint)
        # self.l_pose_sub = rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback)
        self.l_move_base_client = SimpleActionClient('move_base', MoveBaseAction)

        self.m_cancel_pub_ = rospy.Publisher("/move_base/cancel", GoalID, queue_size=1)
        self.m_message_pub_ = rospy.Publisher("/target_message", String, queue_size=1)

        self.m_run_times_ = 0
        self.m_current_target_point_num_ = None
        self.m_current_target_point_key_ = None
        self.m_current_target_point_goal_id_ = None
        self.m_current_target_point_ = None
        self.m_task_status_ = TASK_STATUS.IDLE
        self.m_robot_status_ = ROBOT_STATUS.ROBOT_IDLE
        self.m_robot_broadcast_start_time_ = 0

        self.task_timer = QTimer()
        self.task_timer.timeout.connect(self.task_thread)
        self.task_timer.start(100)
        self.last_message = ""
        self.msg_dict = {}
        self.goal_publish_times = 0

        plan_url = "/move_base_node/NavfnROS/make_plan"
        rospy.wait_for_service(plan_url)
        self.make_plan_client = rospy.ServiceProxy(plan_url, GetPlan)

        rospy.loginfo("l_Waiting for move_base action server...")
        self.l_move_base_client.wait_for_server()

        self.l_pose_array = PoseArray()
        self.l_pose_array.header.frame_id = 'map'
        self.l_pose_index = 0
        # self.l_path_pub = rospy.Publisher('/move_base/NavfnROS/plan', Path, queue_size=10)


        self.l_path = Path()

    # 机器人定位，移动的时候会通过此话题上报自己的坐标
    def amcl_pose_callback(self, msg):
        print(msg.pose.pose.position)
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        z = msg.pose.pose.position.z
        rospy.loginfo("Robot position: x={%f}, y={%f}, z={%f}" % (x, y, z))

    def create_pose_stamped_from_point_stamped(self, point_stamped):
        pose_stamped = PoseStamped()
        pose_stamped.header = point_stamped.header
        pose_stamped.pose.position = point_stamped.point
        pose_stamped.pose.orientation.w = 1.0
        return pose_stamped

    def handleClickedPoint(self, data):
        # print(data)
        pose_stamped = self.create_pose_stamped_from_point_stamped(data)
        self.l_pose_array.poses.append(pose_stamped.pose)
        self.mark_point(pose_stamped)
        self.append_point_to_table(pose_stamped.pose)

    def append_point_to_table(self, pose):
        rospy.loginfo("append to table: x: %f, y: %f, z: %f" % (pose.position.x, pose.position.y, pose.position.z))
        pass

    def mark_point(self, pose_stamped):
        arrow = Marker()
        number = Marker()
        arrow.header.frame_id = number.header.frame_id = pose_stamped.header.frame_id
        arrow.ns = "navi_point_arrow"
        number.ns = "navi_point_number"
        arrow.action = number.action = Marker.ADD
        arrow.type = Marker.ARROW
        number.type = Marker.TEXT_VIEW_FACING
        arrow.pose = number.pose = pose_stamped.pose
        number.pose.position.z += 1.0
        arrow.scale.x = 1.0
        arrow.scale.y = 0.2
        number.scale.z = 1.0
        arrow.color.r = number.color.r = 1.0
        arrow.color.g = number.color.g = 0.98
        arrow.color.b = number.color.b = 0.80
        arrow.color.a = number.color.a = 1.0
        arrow.id = number.id = len(self.l_pose_array.poses)
        number.text = str(len(self.l_pose_array.poses))
        self.l_marker_pub_.publish(arrow)
        self.l_marker_pub_.publish(number)
        rospy.loginfo("point and arrow published: %s" % number.text)

    # 方法可以实现rviz的Publish Point按钮点击就让机器人导航到点击的点
    # def handleClickedPoint(self, data):
    #     # self.l_move_base_client.cancel_all_goals()  # 取消之前的导航目标，注意如果send_goal之后调用了wait_for_result，那么将无法取消上次导航
    #
    #     print(data.point)
    #     rospy.loginfo("x: %f, y: %f, z: %f" % (data.point.x, data.point.y, data.point.z))
    #     goal = MoveBaseGoal()
    #     goal.target_pose.header.frame_id = "map"
    #     goal.target_pose.header.stamp = rospy.Time.now()
    #     goal.target_pose.pose.position = data.point
    #     goal.target_pose.pose.orientation = target_quat
    #
    #     # 发送目标点信息，等待机器人完成导航
    #     rospy.loginfo("Sending goal...")
    #     self.l_move_base_client.send_goal(goal)
    #     # self.l_move_base_client.wait_for_result()
    #     rospy.loginfo("state: %s, status_text: %s" % (self.l_move_base_client.get_state(), self.l_move_base_client.get_goal_status_text()))
    # 打印导航结果
    # if self.l_move_base_client.get_state() == 3:
    #     rospy.loginfo("Goal reached!")
    # else:
    #     rospy.loginfo("Failed to reach goal.")

    def try_append_line(self, text):
        if self.last_message != text:
            self.last_message = text
            self.plainTextEdit.appendPlainText("robot: %s" % text)

    def goal_status_callback(self, msg):
        for m in msg.status_list:
            # m的结构类似：
            # goal_id:
            #     stamp:
            #         secs: 1630265532
            #         nsecs: 985997974
            #     id: 'goal_2'
            # status: 3
            # text: ''
            # rospy.loginfo(self.m_current_target_point_goal_id_)
            # rospy.loginfo(m.goal_id.id)
            if self.m_current_target_point_goal_id_ == m.goal_id.id:
                if ROBOT_STATUS.ROBOT_FINISH == self.m_robot_status_ or ROBOT_STATUS.ROBOT_IDLE == self.m_robot_status_:
                    return

                # PENDING = 0：导航目标已经被发送给move_base节点，但是还没有开始执行。
                # ACTIVE = 1：move_base节点正在执行导航目标。
                # PREEMPTED = 2：导航目标被中止。
                # SUCCEEDED = 3：导航目标已经成功完成。
                # ABORTED = 4：导航目标因某种原因失败。
                # REJECTED = 5：导航目标被move_base节点拒绝执行。
                # PREEMPTING = 6：move_base节点正在取消一个正在执行的导航目标。
                # RECALLING = 7：导航目标已经被取消，正在回收到客户端。
                # RECALLED = 8：导航目标已经被成功地回收到客户端。
                # LOST = 9：导航目标丢失。
                if 3 == m.status:
                    if rospy.Time.now().to_sec() - self.m_robot_broadcast_start_time_ > 10.0:
                        if ROBOT_STATUS.ROBOT_MOVING == self.m_robot_status_:
                            self.m_robot_broadcast_start_time_ = rospy.Time.now().to_sec()
                            self.m_robot_status_ = ROBOT_STATUS.ROBOT_BROADCAST
                        else:
                            self.m_robot_status_ = ROBOT_STATUS.ROBOT_IDLE
                            self.m_current_target_point_num_ = self.m_current_target_point_num_ + 1
                            if self.m_current_target_point_num_ < len(self.m_navigation_lists_):
                                self.m_current_target_point_key_ = self.m_navigation_lists_[
                                    self.m_current_target_point_num_]
                                self.m_current_target_point_goal_id_ = str(
                                    self.m_run_times_) + "_" + self.m_current_target_point_key_ + "_" + str(
                                    self.m_current_target_point_num_)
                                self.m_current_target_point_ = self.m_target_point_maps_[
                                    self.m_current_target_point_key_]
                            else:
                                self.m_robot_status_ = ROBOT_STATUS.ROBOT_FINISH

        # rospy.loginfo("-------")

    def task_thread(self):
        if self.m_task_status_ == TASK_STATUS.START:
            s = String()
            output_string = ""
            if ROBOT_STATUS.ROBOT_IDLE == self.m_robot_status_:
                goal = MoveBaseActionGoal()
                goal.header.frame_id = "map"
                goal.header.stamp = rospy.Time.now()
                goal.goal_id.id = self.m_current_target_point_goal_id_
                rospy.loginfo("send next: %s" % self.m_current_target_point_goal_id_)
                goal.goal.target_pose.pose.position.x = self.m_current_target_point_.x
                goal.goal.target_pose.pose.position.y = self.m_current_target_point_.y
                goal.goal.target_pose.pose.position.z = self.m_current_target_point_.z
                goal.goal.target_pose.pose.orientation.x = self.m_current_target_point_.quat_x
                goal.goal.target_pose.pose.orientation.y = self.m_current_target_point_.quat_y
                goal.goal.target_pose.pose.orientation.z = self.m_current_target_point_.quat_z
                goal.goal.target_pose.pose.orientation.w = self.m_current_target_point_.quat_w
                goal.goal.target_pose.header.frame_id = "map"
                self.goal_publish_times = self.goal_publish_times + 1
                self.m_goal_pub_.publish(goal)
                print("Goal published: %d" % self.goal_publish_times)
                self.m_robot_status_ = ROBOT_STATUS.ROBOT_MOVING
            elif ROBOT_STATUS.ROBOT_BROADCAST == self.m_robot_status_:
                output_string = "I am reached " + self.m_current_target_point_key_ + " and say: \n" + \
                                self.m_target_point_maps_[self.m_current_target_point_key_].message
                msg = self.msg_dict[self.m_current_target_point_key_] if len(
                    self.msg_dict[self.m_current_target_point_key_]) > 0 else self.m_target_point_maps_[
                    self.m_current_target_point_key_].message
                self.try_append_line("我已经到达目标点 %s，%s" % (self.m_current_target_point_key_, msg))
            elif ROBOT_STATUS.ROBOT_FINISH == self.m_robot_status_:
                self.m_task_status_ = TASK_STATUS.IDLE
                self.m_robot_status_ = ROBOT_STATUS.ROBOT_IDLE

            if "" == output_string:
                output_string = "Going to " + self.m_current_target_point_key_
                self.try_append_line("我出发了，目标点 %s" % self.m_current_target_point_key_)

            s.data = output_string
            self.m_message_pub_.publish(s)

        elif self.m_task_status_ == TASK_STATUS.IDLE:
            s = String()
            s.data = "idle"
            self.m_message_pub_.publish(s)
            self.try_append_line("我现在空闲了")
        elif self.m_task_status_ == TASK_STATUS.PAUSE:
            pass
        elif self.m_task_status_ == TASK_STATUS.STOP:
            self.m_current_target_point_num_ = 0
            self.m_current_target_point_key_ = self.m_navigation_lists_[self.m_current_target_point_num_]
            self.m_current_target_point_goal_id_ = str(
                self.m_run_times_) + "_" + self.m_current_target_point_key_ + "_" + str(
                self.m_current_target_point_num_)
            self.m_current_target_point_ = self.m_target_point_maps_[self.m_current_target_point_key_]

    def stop_nav(self):
        rospy.loginfo("Button stop pressed.")
        if self.l_pose_index != 0:
            self.l_move_base_client.cancel_goal()
            return

        cancel_msgs = GoalID()
        cancel_msgs.stamp = rospy.Time.now()
        cancel_msgs.id = self.m_current_target_point_goal_id_
        self.m_cancel_pub_.publish(cancel_msgs)

        self.m_current_target_point_num_ = 0
        self.m_current_target_point_key_ = self.m_navigation_lists_[self.m_current_target_point_num_]
        self.m_current_target_point_goal_id_ = str(
            self.m_run_times_) + "_" + self.m_current_target_point_key_ + "_" + str(self.m_current_target_point_num_)
        self.m_current_target_point_ = NavControl.m_target_point_maps_[self.m_current_target_point_key_]
        self.m_task_status_ = TASK_STATUS.IDLE
        self.m_robot_status_ = ROBOT_STATUS.ROBOT_IDLE

    def l_nav_clear_up(self):
        rospy.loginfo("All navigation point has executed, now delete all navigation marker text and arrow.")
        self.delete_marker()
        self.l_pose_index = 0

    def l_try_nav_to_current_pose(self):
        if self.l_pose_index >= len(self.l_pose_array.poses):
            self.l_nav_clear_up()
            return

        rospy.loginfo("Navigating to %d." % (self.l_pose_index + 1))
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = "map"
        goal.target_pose.pose = self.l_pose_array.poses[self.l_pose_index]
        self.l_pose_index += 1
        self.l_move_base_client.send_goal(goal, done_cb=self.l_done_cb)

    def l_done_cb(self, state, result):
        if state == GoalStatus.PREEMPTED:
            rospy.loginfo("Navigating to %d canceled, result: %s" % (self.l_pose_index, result))
            if self.l_pose_index >= len(self.l_pose_array.poses):
                self.l_nav_clear_up()
                return

        if state == GoalStatus.SUCCEEDED:
            self.l_try_nav_to_current_pose()

    def l_paint_path_line(self):
        if len(self.l_pose_array.poses) == 0:
            return

        del self.l_path.poses[:]
        for index, pose in enumerate(self.l_pose_array.poses):
            start_pose = self.l_pose_array.poses[index]
            next_index = 0 if index == len(self.l_pose_array.poses) - 1 else index + 1
            goal_pose = self.l_pose_array.poses[next_index]

            request = GetPlanRequest()

            request.start.header.frame_id = "map"
            request.start.header.stamp = rospy.Time.now()
            request.start.pose = start_pose

            request.goal.header.frame_id = "map"
            request.goal.header.stamp = rospy.Time.now()
            request.goal.pose = goal_pose

            # print(start_pose, goal_pose)

            resp = self.make_plan_client(request)
            rospy.loginfo("l plan: %d to %d, poses len: %d" % (index, next_index, len(resp.plan.poses)))
            for i in resp.plan.poses:
                self.l_path.poses.append(i)

        self.l_path.header.frame_id = 'map'
        self.l_path.header.stamp = rospy.Time.now()
        self.m_global_path_pub_.publish(self.l_path)
        # self.l_path_pub.publish(self.l_path)

    def start_l_nav(self):
        if len(self.l_pose_array.poses) == 0:
            return

        self.l_paint_path_line()
        self.l_try_nav_to_current_pose()

    def delete_marker(self):
        marker_delete = Marker()
        marker_delete.action = Marker.DELETEALL
        self.l_marker_pub_.publish(marker_delete)
        del self.l_pose_array.poses[:]

    def start_nav(self):
        point_count = len(self.l_pose_array.poses)
        if point_count > 0:
            self.start_l_nav()
            return
        self.goal_publish_times = 0
        rospy.loginfo("Button start pressed.")
        self.m_run_times_ = self.m_run_times_ + 1
        self.m_current_target_point_num_ = 0
        self.m_current_target_point_key_ = self.m_navigation_lists_[self.m_current_target_point_num_]
        self.m_current_target_point_goal_id_ = str(
            self.m_run_times_) + "_" + self.m_current_target_point_key_ + "_" + str(self.m_current_target_point_num_)
        self.m_current_target_point_ = self.m_target_point_maps_[self.m_current_target_point_key_]
        self.m_task_status_ = TASK_STATUS.START

        # self.plainTextEdit.clear()

    def parse_nav_path(self, point_list):
        del self.m_navigation_lists_[:]
        del self.m_global_path_.poses[:]
        for p in point_list:
            self.m_navigation_lists_.append(p["name"])
            self.msg_dict[p["name"]] = p["message"]

        points = self.m_navigation_lists_

        for index, point_name in enumerate(points):
            it = NavControl.m_target_point_maps_[point_name]
            end_point_name = points[index + 1] if index < len(points) - 1 else points[0]
            end = NavControl.m_target_point_maps_[end_point_name]
            request = GetPlanRequest()
            request.start.header.frame_id = "map"
            request.start.header.stamp = rospy.Time.now()
            request.start.pose.position.x = it.x
            request.start.pose.position.y = it.y
            request.start.pose.position.z = it.z
            request.start.pose.orientation.x = it.quat_x
            request.start.pose.orientation.y = it.quat_y
            request.start.pose.orientation.z = it.quat_z
            request.start.pose.orientation.w = it.quat_w

            request.goal.header.frame_id = "map"
            request.goal.header.stamp = rospy.Time.now()
            request.goal.pose.position.x = end.x
            request.goal.pose.position.y = end.y
            request.goal.pose.position.z = end.z
            request.goal.pose.orientation.x = end.quat_x
            request.goal.pose.orientation.y = end.quat_y
            request.goal.pose.orientation.z = end.quat_z
            request.goal.pose.orientation.w = end.quat_w
            resp = self.make_plan_client(request)
            rospy.loginfo("(%s) to (%s) called, poses len: %d" % (point_name, end_point_name, len(resp.plan.poses)), )
            for i in resp.plan.poses:
                self.m_global_path_.poses.append(i)

        rospy.loginfo("length of m_global_path_.poses: %d" % len(self.m_global_path_.poses))
        self.m_navigation_lists_.append(self.m_navigation_lists_[0])

        s = " -> ".join(self.m_navigation_lists_)
        self.try_append_line("路径规划完毕，%s，随时可以出发" % s)

        self.publish_global_path()

    def publish_global_path(self):
        if len(self.m_global_path_.poses) > 0:
            self.m_global_path_.header.frame_id = "map"
            self.m_global_path_.header.stamp = rospy.Time.now()
            self.m_global_path_pub_.publish(self.m_global_path_)
            rospy.loginfo("m_global_path_ published")

    def load_and_publish(self, path="/home/ubtech/bobac3/src/bobac3_navigation/maps/target_points.yaml"):
        NavControl.m_target_point_maps_ = load_targets(path)
        self.publish_markers(NavControl.m_target_point_maps_)

    def publish_markers(self, targets):
        i = 0
        sorted_items = sorted(targets.items())
        for key, value in sorted_items:
            target = value
            marker = Marker()
            marker.header.stamp = rospy.Time.now()
            marker.header.frame_id = "map"
            marker.type = Marker.POINTS
            marker.action = Marker.ADD
            marker.pose.orientation.w = 1.0
            marker.lifetime = rospy.Duration()
            marker.ns = "target_point"
            marker.id = i
            i = i + 1
            marker.scale.x = 0.2
            marker.scale.y = 0.2
            marker.scale.z = 0.2

            # 设置绿色
            marker.color.r = 0.0
            marker.color.g = 1.0
            marker.color.b = 0.0
            marker.color.a = 1.0

            marker.pose.position.x = target.x
            marker.pose.position.y = target.y
            marker.pose.position.z = 0.02
            pose_marker = Point()
            pose_marker.x = 0
            pose_marker.y = 0
            pose_marker.z = 0.3
            marker.points.append(pose_marker)
            self.m_visualization_markers_pub_.publish(marker)

            marker_text = Marker()
            marker_text.header.stamp = rospy.Time.now()
            marker_text.header.frame_id = "map"
            marker_text.type = Marker.TEXT_VIEW_FACING
            marker_text.action = Marker.ADD
            marker_text.pose.orientation.w = 1.0
            marker_text.lifetime = rospy.Duration()
            marker_text.ns = "target_point"

            marker_text.id = i
            i = i + 1
            marker_text.scale.x = 1.0
            marker_text.scale.y = 1.0
            marker_text.scale.z = 1.0

            # 设置红色
            marker_text.color.r = 1.0
            marker_text.color.g = 0.0
            marker_text.color.b = 0.0
            marker_text.color.a = 1.0

            p = np.array([0.8, 0, 0])
            w, x, y, z = target.quat_w, target.quat_x, target.quat_y, target.quat_z
            q = np.array([w, x, y, z])
            qm = matrix(q)
            p_new = np.dot(qm, p)
            marker_text.pose.position.x = target.x - p_new[0]
            marker_text.pose.position.y = target.y - p_new[1]
            marker_text.pose.position.z = 0.02
            marker_text.text = key
            rospy.loginfo("%s marker and text published" % key)
            self.m_visualization_markers_pub_.publish(marker_text)

        self.try_append_line("我已经知晓场地里所有导览点，告诉我要先后去哪些导览点吧")
        return targets
